
// openxvario http://code.google.com/p/openxvario/
// started by R.Schloßhan 

/***************************************************************************************/
/* sensitivity / data filtering / Kalman filter parameters                             */
/* The Kalman Filter is being used to remove noise from the MS5611 sensor data. It can */
/* be adjusted by changing the value Q (process Noise) and R (Sensor Noise).           */
/* Best pactise is to only change R, as both parametrers influence each other.         */
/* (less Q requires more R to compensate and vica versa)                               */
/***************************************************************************************/
#define KALMAN_Q 0.05 // 0.05 do not tamper with this value unless you are looking for adventure ;-)
#define KALMAN_R 300  // default:300   change this value if you want to adjust the default sensitivity!
                      // this will only be used if PIN_PPM is NOT defined
                      //  50 = fast but lot of errors (good for sensor testing...but quite a lot of noise)
                      // 300 = medium speed and less false indications (nice medium setting with 
                      //       a silence window (indoor)of -0.2 to +0.2)
                      // 1000 = conservative setting ;-)
                      // 500 = still useable maybe for a slow flyer?
                      // 1000 and q=0.001 is it moving at all?? in stable airpressure you can measure 
                      //      the height of your furniture with this setting. but do not use it for flying ;-)
                      // .. everything inbetween in relation to these values.

#define OFFSET_PRESSURE					(0)
#define OFFSET_TEMPERATURE				(-80)

#define QUEUE_SIZE_PRESSURE				(6)
#define QUEUE_SIZE_CLIMBRATE			(10)
 
typedef struct tagVARIOSTATE
{
	// Pressure
	long			pressure_queue[QUEUE_SIZE_PRESSURE];
	char			pressure_index;
	long			pressure_current;
	
	// Climb Rate
	float			climbrate_queue[QUEUE_SIZE_CLIMBRATE];
	char			climbrate_index;
	float			climbrate_current;
	
	//
	unsigned long	timer_last;
	unsigned long	timer_climbrate;

	//
	float			altitute_current;	
	float			altitute_last;

	//	
	float			temperature_last;
	
//	int				deviation;
	
	double			kalman_q;
	double			kalman_r;
	
} VarioState;

VarioState	v_state;
  
/********************************************************************************** Setup() */
void setup()
{
	int i;
	
	// setup serial
	// setup sensor
	// setup timer
	
	// setup state
	v_state.kalman_q = KALMAN_Q;
	v_state.kalman_r = KALMAN_R;
	
	//
	v_state.pressure_index = 0;
	
	for( i = 0; i < QUEUE_SIZE_PRESSURE * 2; i++ )
		savePressure(getPressure());
	
	//	
	v_state.climbrate_index = 0;
	
	for( i = 0; i < QUEUE_SIZE_CLIMBRATE; i++ )
		v_state.climbrate_queue[i] = 0.0; // saveClimbRate(0);

	//
	v_state.pressure_current = getAveragePressure();
	v_state.altitute_current = getAltitude(v_state.pressure_current, v_state.temperature_last + TEMPERATURE_CALIBRATION_OFFSET);
	v_state.altitute_last	 = v_state.altitute_current;
	v_state.climbrate_current = 0; // getClimbRate();	

	v_state.timer_climbrate = v_state.timer_last = timer0_get_tick();
}

void loop()
{
	long pressure;
	float altitute;
	
	while( 1 )
	{
		// get the measurements & calculate climb rate
		pressure = getPressure();		
		altitute = getAltitude(pressure, v_state.temperature_last + TEMPERATURE_CALIBRATION_OFFSET);
		altitute = kalman_update(altitute);

		savePressure(pressure);
		saveClimbRate(altitute);
  
		if( v_state.timer_last + 200 <= timer0_get_tick() ) // every 200ms
		{
			v_state.timer_last = timer0_get_tick();
	  
			v_state.pressure_current = getAveragePressure();
			v_state.altitute_current = getAltitude(v_state.pressure_current, v_state.temperature_last + TEMPERATURE_CALIBRATION_OFFSET);
			v_state.climbrate_current = getClimbRate();
	  
			#ifdef DEBUG
			Serial.print(" Pressure:");    Serial.print(pressure,DEC);
			Serial.print(" AveragePressure:");    Serial.print((float)avgPressure/100,DEC);
			Serial.print(" Altitude:");      Serial.print(alt/100,DEC);
			Serial.print(" ClimbRate:");      Serial.print(climbRate,DEC);
			Serial.print(" K_R:");    Serial.print(kalman_r);
			Serial.print(" Temp:");    Serial.print(Temp,DEC);
			Serial.print(" VCC:"); Serial.print(readVccMv());
			Serial.println();
			#endif	  
		}
  
		/*
		// Frame 2 to send every 1000ms (must be this intervall, otherwise the climb rate calculation in open9x will not work 
		if( (lastMillisFrame2 + 1000) <=timer0_get_tick()) 
		{
		float avgTemp=getAverageTemperature(0);
		SendGPSAlt(getAltitude(avgPressure,avgTemp));
    
		mySerial.write(0x5E); // End of Frame 1!
		lastMillisFrame2=timer0_get_tick(); 
		}
		*/
	}	  
}

void saveClimbRate(float altitute)
{
	unsigned long now = timer0_get_tick();
	unsigned long duration = now - v_state.timer_climbrate;

	if( duration > 0 )
	{	
		float climbrate = (float)(altitute - v_state.altitute_last) * 1000.0 / (float)duration;
	
		v_state.climbrate_queue[v_state.climbrate_index] = climbrate;
		v_state.climbrate_index = (v_state.climbrate_index + 1) % QUEUE_SIZE_CLIMBRATE;
		
		v_state.timer_climbrate = now;
		v_state.altitute_last = altitute;
	}
}

/* retrieve the average climbRate */
float getClimbRate()
{
	float climbrate = 0;
	int i;
	
	for( i = 0; i < QUEUE_SIZE_CLIMBRATE; i++ )
		climbrate += v_state.climbrate_queue[i];
		
	climbrate /= QUEUE_SIZE_CLIMBRATE;
	
	return climbrate;
}

/****************************************************************/
/* getAltitude - convert mbar and temperature into an altitude  */
/*               value                                          */
/****************************************************************/
float getAltitude(long pressure, int temperature) 
{
	const float sea_pressure = 1013.25;
	
	return (((pow((sea_pressure / ((float)pressure / 100.0)), 1 / 5.257) - 1.0) * (((float)temperature/1000.0 + 273.15)) / 0.0065 * 100.0);	
	
	/*
	float   result;
	temp/=10;
	result=(float)press/100;
	const float sea_press = 1013.25;

	return (((pow((sea_press / result), 1/5.257) - 1.0) * ( (temp/100) + 273.15)) / 0.0065 *100);
	*/
}


/****************************************************************/
/* SavePressure - save a new pressure value to the buffer       */
/* Rotating Buffer for calculating the Average Pressure         */
/****************************************************************/
void savePressure(long pressure)
{
	v_state.pressure_queue[v_state.pressure_index] = pressure;
	v_state.pressure_index = (v_state.pressure_index + 1) % QUEUE_SIZE_PRESSURE;
}

/****************************************************************/
/* getAveragePress - calculate average pressure based on all    */
/* entries in the pressure buffer                               */
/****************************************************************/
long getAveragePressure()
{
	long pressure = 0;
	int i;
	
	for( i = 0; i < QUEUE_SIZE_PRESSURE; i++ )
		pressure += v_state.pressure_queue[i];
		
	return pressure / QUEUE_SIZE_PRESSURE;
}

/****************************************************************/
/* getPressure - Read pressure + temperature from the MS5611    */
/****************************************************************/
long getPressure()
{
	/*
	long D1, D2, dT, P;
	float TEMP;
	int64_t OFF, SENS;

	//  D1 = getData(0x48, 10); //OSR=4096 0.012 mbar precsision
	D1 = getData(0x4096 0.012 mbar precsision
	D2 = getData(0x50, 1);

	dT = D2 - ((long)calibrationData[5] << 8);
	TEMP = (2000 + (((int64_t)dT * (int64_t)calibrationData[6]) >> 23)) / (float)100;
	Temp=(int)(TEMP*10);
	OFF = ((unsigned long)calibrationData[2] << 16) + (((int64_t)calibrationData[4] * dT) >> 7);
	SENS = ((unsigned long)calibrationData[1] << 15) + (((int64_t)calibrationData[3] * dT) >> 8);
	P = (((D1 * SENS) >> 21) - OFF) >> 15;

	return (P + PressureCalibrationOffset); 
  
	//return P+475;
	//return P+1762;
	*/
	
	unsigned long p;
	unsigned short t;
	
	t = bmp085_calc_temperature(bmp085_measure_temperature());
	p = bmp085_calc_pressure(bmp085_measure_pressure());
	
	v_state.temperature_last = t;

	return p;
}


float kalman_update(float measurement)
{
	static float x = v_state.altitute_current;	// value
	static float p = 100;	// estimation error covariance
	static float k = 0;		// kalman gain
  
	// update the prediction value
	p = p + v_state.kalman_q;

	// update based on measurement
	k = p / (p + v_state.kalman_r);
	x = x + k * (measurement - x);
	p = (1 - k) * p;
  
	/*
	deviation = measurement - kalman_state.x;
	Serial.print("q=");Serial.print(q);  Serial.print(" r=");Serial.print(r);
	Serial.print(" p=");  Serial.print(p);  Serial.print(" X=");Serial.print(x);
	Serial.print(" k=");Serial.print(k);  Serial.print(" D=");Serial.print(deviation );
	Serial.println();
	*/

	return x;
}
